#include "a.hpp"

#define ENEMYINFO 2//red==2 blue==0


void cv_show(const cv::Mat &img) {
	cv::imshow("img", img);
	cv::waitKey(0);
}

ArmorDetector::ArmorDetector() {}

cv::Mat ArmorDetector::HSV_img(cv::Mat &img) {
	
	cv::cvtColor(img, imgHSV, cv::COLOR_BGR2HSV);
	cv::split(imgHSV, channels);
	cv::minMaxLoc(channels.at(2), nullptr, &maxValue);
	cv::threshold(channels.at(2), imgBinary, maxValue * 0.98, 255, cv::THRESH_BINARY);
	cv::Mat element = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
	morphologyEx(imgBinary, HSV, cv::MORPH_DILATE, element, cv::Point(-1, -1), 3);
	return HSV;
	
}

cv::Mat ArmorDetector::BGR_img(cv::Mat &img) {
	cv::split(img.clone(), channels);
#if ENEMYINFO == 0
	cv::subtract(channels.at(0),channels.at(2),imgBinary);
#elif ENEMYINFO == 2
	cv::subtract(channels.at(2), channels.at(0), imgBinary);
#endif
	cv::threshold(imgBinary, imgBinary, 0, 255, cv::THRESH_BINARY_INV);
	
	cv::Mat element = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(9, 9));
	morphologyEx(imgBinary, BGR, cv::MORPH_DILATE, element, cv::Point(-1, -1), 2);
	return BGR;
	
}

std::vector <std::vector<cv::Point>> ArmorDetector::contourProcess() {
	cv::Mat Hsv = HSV_img(src);
	cv_show(Hsv);
	cv::Mat Bgr = BGR_img(src);
	cv_show(Bgr);
	cv::bitwise_and(Hsv, Bgr, ProcessedImg);
	cv_show(ProcessedImg);
	findContours(ProcessedImg, light_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
	return light_contours;
}

cv::RotatedRect &adjustRec(cv::RotatedRect &rec)//矫正灯条
{
	
	float &width = rec.size.width;
	float &height = rec.size.height;
	float &angle = rec.angle;
	
	
	while (angle >= 90.0) angle -= 180.0;
	while (angle < -90.0) angle += 180.0;
	
	
	if (angle >= 45.0) {
		std::swap(width, height);
		angle -= 90.0;
	} else if (angle < -45.0) {
		std::swap(width, height);
		angle += 90.0;
	}
	
	
	return rec;
}

void drawall(const std::vector <cv::RotatedRect> &rec, cv::Mat img) {
	for (auto &i: rec) {
		cv::Point2f p[4];
		i.points(p);
		line(img, p[0], p[1], cv::Scalar(0, 0, 255), 1, 8, 0);
		line(img, p[1], p[2], cv::Scalar(0, 0, 255), 1, 8, 0);
		line(img, p[2], p[3], cv::Scalar(0, 0, 255), 1, 8, 0);
		line(img, p[3], p[0], cv::Scalar(0, 0, 255), 1, 8, 0);
	}
}

std::vector <cv::RotatedRect> ArmorDetector::detection() {
	armor_info.clear();
	ellipse.clear();
	auto cnts = contourProcess();
//select contours
	for (const auto &cnt: cnts) {
		cv::Mat dst = src.clone();
		if (cnt.size() < 10) continue;
		cv::RotatedRect temp = cv::fitEllipse(cnt);
		auto external_rect = temp.boundingRect();
		auto ell_rect_area = temp.size.area();
		double area = external_rect.area();
		//delete small area
		if (!(ell_rect_area > 800 && ell_rect_area < 15000 && external_rect.width > 0 &&
			  external_rect.height > 0))
			continue;
//		select from x_dis
		float x = temp.center.x - temp.size.width;
		if (x < 0)
			continue;
//				select from area ratio
		
		if (area / cv::fitEllipse(cnt).size.area() < 1) continue;
//		select from height
		if (temp.size.height < 10 || temp.size.height > 300) continue;
		
//		draw bounding Rect
//		cv::rectangle(src,external_rect.tl(),external_rect.br(),cv::Scalar(0,212,34),2);
//		std::cout<<area<<std::endl;
//		std::cout<<temp.size.height<<"  "<<temp.size.width<<std::endl;
//		cv_show(dst);

		ellipse.push_back(adjustRec(temp));

		
	}
//sort form bottom to top
	sort(ellipse.begin(), ellipse.end(), [](const cv::RotatedRect &ld1, const cv::RotatedRect &ld2) {
		return ld1.center.y > ld2.center.y;
	});

	cv_show(src);
	std::cout << "轮廓检索完毕" << std::endl;
	
//	armor Match
	for (int i = 0; i < ellipse.size(); i++) {
		
		for (int j = i + 1; j < ellipse.size(); j++) {
			const cv::RotatedRect &left = ellipse[i];
			const cv::RotatedRect &top = ellipse[j];
			
			float angleDiff = abs(left.angle - top.angle);
			float dis = sqrt((left.center.x - top.center.x) * (left.center.x - top.center.x) +
							 (left.center.y - top.center.y) * (left.center.y - top.center.y));
			float meanheight = (left.size.height + top.size.height) / 2;
//			dis_ratio parse
			ratio = dis / meanheight;
			std::cout << "here" << std::endl;
			std::cout << "left:" << left.angle << "   " << "right: " << top.angle << "    " << angleDiff << std::endl;
			std::cout << "ratio:\t" << ratio << std::endl;
			if (ratio < 1)continue;
			if (dis > 550 || dis < 200) continue;
			armor_info.push_back(ellipse[i]);
			armor_info.push_back(ellipse[j]);
			
			if (armor_info.size() == 2)break;
//			cv::Mat T=src.clone();
//			std::cout<<dis<<std::endl;
//			drawall(armor_info,T);
//			cv_show(T);
//			armor_info.clear();

		}
		if (armor_info.size() == 2)break;
		
	}
	std::cout << armor_info.size() << std::endl;
//	draw armor info
	drawall(armor_info, src);
	cv_show(src);
	
	InfoParse();
	return armor_info;
//	cv::Mat T=src.clone();
}

void ArmorDetector::InfoParse() {
	cv::RotatedRect armor;
	armor.center = (armor_info[0].center + armor_info[1].center) / 2;
	double angle1 = (armor_info[0].angle + armor_info[1].angle) / 2;
//	double k = (armor_info[0].center.y - armor_info[1].center.y) / (armor_info[0].center.x - armor_info[1].center.x);
	
	armor.angle = angle1 + 0;
//	std::cout << "this is armor angle " << angle1 << std::endl;
//	std::cout << "this os atan " << k << std::endl;
	armor.size.height = (armor_info[0].size.height + armor_info[1].size.height);
	armor.size.width = sqrt(
			(armor_info[0].center.x - armor_info[1].center.x) * (armor_info[0].center.x - armor_info[1].center.x) +
			(armor_info[0].center.y - armor_info[1].center.y) * (armor_info[0].center.y - armor_info[1].center.y));

	if (ratio > 7) {
		armor.size.height *= 4;
		std::swap(armor.size.width, armor.size.height);
	}
//	cv::Rect armor_rect = armor.boundingRect();

	cv::circle(src, armor.center, 5, cv::Scalar(222, 44, 5), 2);
	cv::circle(src, armor_info[0].center, 5, cv::Scalar(0, 44, 255), 2, cv::LINE_8);
	cv::circle(src, armor_info[1].center, 5, cv::Scalar(222, 44, 5), 2);
	cv::line(src, armor_info[1].center, armor_info[0].center, cv::Scalar(222, 44, 5), 2);
	cv_show(src);
	std::cout << armor.angle << std::endl;
	cv::Point2f p[4];
	armor.points(p);//返回四个点的值
	line(src, p[0], p[1], cv::Scalar(0, 0, 255), 1, 8, 0);
	line(src, p[1], p[2], cv::Scalar(0, 0, 255), 1, 8, 0);
	line(src, p[2], p[3], cv::Scalar(0, 0, 255), 1, 8, 0);
	line(src, p[3], p[0], cv::Scalar(0, 0, 255), 1, 8, 0);
	
	cv_show(src);
}
